#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>

/* Scheduler include files. */
#include "FreeRTOS.h"
#include "task.h"
#include "projdefs.h"
#include "semphr.h"

/* include files. */
#include "vtUtilities.h"
#include "vtI2C.h"
#include "LCDtask.h"
#include "i2cAdc.h"
#include "I2CTaskMsgTypes.h"

/* *********************************************** */
// definitions and data structures that are private to this file
// Length of the queue to this task
#define vtAdcQLen 10 
// actual data structure that is sent in a message
typedef struct __vtAdcMsg {
	uint8_t msgType;
	uint8_t	length;	 // Length of the message to be printed
	uint8_t buf[vtAdcMaxLen+1]; // On the way in, message to be sent, on the way out, message received (if any)
} vtAdcMsg;

// I have set this to a large stack size because of (a) using printf() and (b) the depth of function calls
//   for some of the i2c operations	-- almost certainly too large, see LCDTask.c for details on how to check the size
#define baseStack 3
#if PRINTF_VERSION == 1
#define i2cSTACK_SIZE		((baseStack+5)*configMINIMAL_STACK_SIZE)
#else
#define i2cSTACK_SIZE		(baseStack*configMINIMAL_STACK_SIZE)
#endif

// end of defs
/* *********************************************** */

/* The i2cTemp task. */
static portTASK_FUNCTION_PROTO( vvi2cAdcUpdateTask, pvParameters );

/*-----------------------------------------------------------*/
// Public API
void vStarti2cAdcTask(vtAdcStruct* params, unsigned portBASE_TYPE uxPriority, vtI2CStruct *i2c)
{
	// Create the queue that will be used to talk to this task
	if ((params->inQ = xQueueCreate(vtAdcQLen,sizeof(vtAdcMsg))) == NULL) {
		VT_HANDLE_FATAL_ERROR(0);
	}
	/* Start the task */
	portBASE_TYPE retval;
	params->dev = i2c;
	if ((retval = xTaskCreate( vvi2cAdcUpdateTask, ( signed char * ) "i2cAdc", i2cSTACK_SIZE, (void *) params, uxPriority, ( xTaskHandle * ) NULL )) != pdPASS) {
		VT_HANDLE_FATAL_ERROR(retval);
	}
}

portBASE_TYPE SendAdcTimerMsg(vtAdcStruct* adcData,portTickType ticksElapsed,portTickType ticksToBlock)
{
	if (adcData == NULL) {
		VT_HANDLE_FATAL_ERROR(0);
	}
	vtAdcMsg adcBuffer;
	adcBuffer.length = sizeof(ticksElapsed);
	if (adcBuffer.length > vtAdcMaxLen) {
		// no room for this message
		VT_HANDLE_FATAL_ERROR(adcBuffer.length);
	}
	memcpy(adcBuffer.buf,(char *)&ticksElapsed,sizeof(ticksElapsed));
	adcBuffer.msgType = AdcMsgTypeTimer;
	return(xQueueSend(adcData->inQ,(void *) (&adcBuffer),ticksToBlock));
}

portBASE_TYPE SendSensorValueMsg(vtAdcStruct* adcData, uint8_t msgType,uint8_t* value,portTickType ticksToBlock)
{
	vtAdcMsg adcBuffer;

	if (adcData == NULL) {
		VT_HANDLE_FATAL_ERROR(0);
	}
	adcBuffer.length = sizeof(value);
	if (adcBuffer.length > vtAdcMaxLen) {
		// no room for this message
		VT_HANDLE_FATAL_ERROR(adcBuffer.length);
	}
	memcpy(adcBuffer.buf,(char *)&value,sizeof(value));
	adcBuffer.msgType = msgType;
	return(xQueueSend(adcData->inQ,(void *) (&adcBuffer),ticksToBlock));
}

// End of Public API
/*-----------------------------------------------------------*/
int getMsgType(vtAdcMsg *Buffer)
{
	return(Buffer->msgType);
}
uint8_t getValue(vtAdcMsg *Buffer)
{
	uint8_t *ptr = (uint8_t *) Buffer->buf;
	return(*ptr);
}


	const uint8_t i2cCmdReadSensors[]= {0x4E,0xB1};
	const uint8_t i2cCmdReadEncoders[]= {0x1E, 0xA1};

	const uint8_t fsmStateReadSensors = 0;
	const uint8_t fsmStateReadEncoders = 1;


// This is the actual task that is run
static portTASK_FUNCTION( vvi2cAdcUpdateTask, pvParameters )
{

	// Get the parameters
	vtAdcStruct *param = (vtAdcStruct *) pvParameters;
	// Get the I2C device pointer
	vtI2CStruct *devPtr = param->dev;
	// Buffer for receiving messages
	vtAdcMsg msgBuffer;

	// Assumes that the I2C device (and thread) have already been initialized

	// This task is implemented as a Finite State Machine.  The incoming messages are examined to see
	//   whether or not the state should change.
	//
	// Temperature sensor configuration sequence (DS1621) Address 0x4F
	/*if (vtI2CEnQ(devPtr,vtI2CMsgTypeAdcInit,0x4F,sizeof(i2cCmdInit),i2cCmdInit,0) != pdTRUE) {
		VT_HANDLE_FATAL_ERROR(0);
	}*/
	/*if (vtI2CEnQ(devPtr,vtI2CMsgTypeSensorRead,0x17,sizeof(i2cCmdReadSensors),i2cCmdReadSensors,4) != pdTRUE) {
		VT_HANDLE_FATAL_ERROR(0);
	}		  */
	//currentState = fsmStateInit1Sent;
	uint8_t currentState = fsmStateReadSensors;
	// Like all good tasks, this should never exit
	for(;;)
	{

		uint8_t rightSens;
		uint8_t leftSens;
		uint8_t frontSens;
		uint8_t finishSensor;
		
		int xCoord = 0;
		int yCoord = 0; 


		// Wait for a message from either a timer or from an I2C operation
		if (xQueueReceive(param->inQ,(void *) &msgBuffer,portMAX_DELAY) != pdTRUE) {
			VT_HANDLE_FATAL_ERROR(0);
		}

		// Now, based on the type of the message and the state, we decide on the new state and action to take
		switch(getMsgType(&msgBuffer)) {
		case AdcMsgTypeTimer: {
			if(currentState == fsmStateReadSensors)
			{
				if (vtI2CEnQ(devPtr,vtI2CMsgTypeSensorRead,0x17,sizeof(i2cCmdReadSensors),i2cCmdReadSensors,4) != pdTRUE) {
					VT_HANDLE_FATAL_ERROR(0);
				}
				currentState = fsmStateReadEncoders;
			}
			else
			{
				if (vtI2CEnQ(devPtr,vtI2CMsgTypeEncoderRead,0x17,sizeof(i2cCmdReadEncoders),i2cCmdReadEncoders,4) != pdTRUE) {
					VT_HANDLE_FATAL_ERROR(0);
				}
				currentState = fsmStateReadSensors;
			}
			break;
		}
		case vtI2CMsgTypeSensorRead: {
			//store data
			rightSens = msgBuffer.buf[0];
			leftSens = msgBuffer.buf[1];
			frontSens = msgBuffer.buf[2];
			finishSensor = msgBuffer.buf[3];
			//send message to driving task here, as appropriate

			break;
		}
		case vtI2CMsgTypeEncoderRead: {
			//store data (obviously this is wrong, it's just for testing)
			yCoord+=msgBuffer.buf[0]-128;
			xCoord+=msgBuffer.buf[1]-128;


			//send message to driving task here, as appropriate

			break;
		}
		default: {
			VT_HANDLE_FATAL_ERROR(getMsgType(&msgBuffer));
			break;
		}
		}


	}
}

